// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URControlCommandBuilderTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.UniversalRobots
{
    using System;
    using Microsoft.Robotics.Hardware.UniversalRobots;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Tests for URControlCommandBuilder
    /// </summary>
    [TestClass]
    public class URControlCommandBuilderTests
    {
        /// <summary>
        /// Test parsing and publishing of feedback from <c>URControlClient</c>
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void CartesianPositionGoalStringTest()
        {
            double[] pos = 
            {
                1,
                1.2,
                1.3,
                2.1,
                2.2,
                -2.3
            };

            string expectedResult = "movej(p[1, 1.2, 1.3, 2.1, 2.2, -2.3])\n";

            string actual = URControlCommandBuilder.ConstructCartesianPositionGoalString(pos);

            Assert.AreEqual(expectedResult, actual);
        }

        /// <summary>
        /// Test parsing and publishing of feedback from <c>URControlClient</c>
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void JointPositionGoalStringTest()
        {
            double[] pos = 
            {
                1,
                1.2,
                1.3,
                -2.3,
                2.1,
                2.2
            };

            double a = 1.4;

            double v = 1.1;

            double r = 0.1;

            string expectedResult = "movej([1, 1.2, 1.3, -2.3, 2.1, 2.2],1.4,1.1,0.1)\n";

            string actual = URControlCommandBuilder.ConstructJointPositionGoalString(pos, a, v, r);

            Assert.AreEqual(expectedResult, actual);
        }

        /// <summary>
        /// Test parsing and publishing of feedback from <c>URControlClient</c>
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void JointVelocityStringTest()
        {
            double[] vel = 
            {
                1,
                1.2,
                1.3,
                -2.3,
                2.1,
                2.2
            };

            double a = 1.4;

            double timeout = 0.3;

            string expectedResult = "speedj([1, 1.2, 1.3, -2.3, 2.1, 2.2],1.4,0.3)\n";

            string actual = URControlCommandBuilder.ConstructJointVelocityString(vel, a, timeout);

            Assert.AreEqual(expectedResult, actual);
        }
    }
}
